#include <stdio.h>
#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <string>

#include <gsl/gsl_math.h>

#include "IOParams.h"

#define DIM 2

int indx(int i, int j){
  return (i*DIM+j);
}

using namespace std;

int main(int argc, char **argv){

  IOParams io(argc, argv);
  cout<<"Params "<<io.N<<" "<<io.T1<<" "<<io.T2<<" "<<io.total_time<<endl;
  const int N = io.N;
  
  double pi = 3.14159;
  double *theta_old, *theta_new;
  double *omega_old, *omega_new;
  double *M_old, *m_old;
  double *M_new, *m_new;
  double *force_old, *force_new;
  
  theta_old = new double[N];
  theta_new = new double[N];
  omega_old = new double[N];
  omega_new = new double[N];
  
  M_old = new double[DIM];
  M_new = new double[DIM];
  m_old = new double[N*DIM];
  m_new = new double[N*DIM];
  force_old = new double[N];
  force_new = new double[N];

  // Initiate angles.
  for (int i=0; i<N ; i++){
    theta_old[i]=(2.0*(i+0.5)/N -1 )*pi ;
    cout<<theta_old[i]<<" ";
  }
  cout<<endl;
  cout<<"----- omega_old-----"<<endl;
  // Initiate velocities.
  for (int i=0; i<N ; i++){
    omega_old[i]=0.0;
    cout<<omega_old[i]<<" ";
  }
  cout<<endl;
  
  
  //----------- Begin simulation   -----------
  // 1. Calculate force_old.
  //--  1.1 Calculate m_old.
  for (int i=0; i< N; i++){
    m_old[indx(i,0)] = cos(theta_old[i]);
    m_old[indx(i,1)] = sin(theta_old[i]);
  }
  
  //--  1.2 Calculate M.
  M_old[0]=0;
  M_old[1]=0;
  for (int i=0; i<N; i++){
    M_old[0] += m_old[indx(i,0)];
    M_old[1] += m_old[indx(i,1)];
  }
  cout<<M_old[0]<<" "<<M_old[1]<<endl;
  M_old[0] = M_old[0]/N;
  M_old[1] = M_old[1]/N;
  
  cout<<M_old[0]<<" "<<M_old[1]<<endl;
  double Mt_old, Mt_new;
  Mt_old = M_old[0]*M_old[0]+M_old[1]*M_old[1];
  cout<< Mt_old <<endl;
  
  //--  1.3 Calculate ___force_old___ with M_old
  for (int i=0; i<N; i++){
    force_old[i] = -io.eps*(M_old[0]*sin(theta_old[i]) - M_old[1]*cos(theta_old[i]));
  }
  
  // 2. Calculate ___theta_new___
  for (int i=0; i<N; i++){
    theta_new[i] = theta_old[i]+omega_old[i]*io.dt+0.5*io.dt2*force_old[i];
  }
  
  // 3. Calculate force_new with M_new
  //--  3.1 Calculate m_new.
  for (int i=0; i< N; i++){
    m_new[indx(i,0)] = cos(theta_new[i]);
    m_new[indx(i,1)] = sin(theta_new[i]);
  }
  //--  3.2 Calculate M_new
  M_new[0]=0;
  M_new[1]=0;
  for (int i=0; i<N; i++){
    M_new[0] += m_new[indx(i,0)];
    M_new[1] += m_new[indx(i,1)];
  }
  M_new[0] = M_new[0]/N;
  M_new[1] = M_new[1]/N;
  
  //--  3.3 Calculate ___force_new___ with M_new
  for (int i=0; i<N; i++){
    force_new[i] = -io.eps*(M_new[0]*sin(theta_new[i]) - M_new[1]*cos(theta_new[i]));
  }
  
  // 4. Calculate velocity ___omega_new___
  for (int i=0; i<N ; i++){
    omega_new[i]=omega_old[i]+ 0.5*io.dt*(force_new[i]+force_old[i]);
  }
  cout<<"----- omega_new-----"<<endl;
  for (int i=0; i<N; i++){
    cout<< omega_new[i] << " ";
  }
  cout<<endl;
  //----------- End simulation   -----------
  
  //Calculate Energy
  ofstream outfile;  
  outfile.open((io.outfilename).c_str());

  // Save data
  for (int i=0; i<N; i++){
    outfile<< theta_new[i]<<" "<<omega_new[i] << " ";
  }
  outfile<<endl;
  
}

